package it.gius.processing.gjkTest;

import java.util.concurrent.Semaphore;

import org.jbox2d.common.Vec2;

import it.gius.pe.SystemCostants;
import it.gius.pe.Shape.BadShapeException;
import it.gius.pe.Shape.AABoundaryBox;
import it.gius.pe.Shape.Ellipse;
import it.gius.pe.Shape.Polygon;
import it.gius.pe.algorithm.gjk.DistanceSolution;
import it.gius.pe.algorithm.gjk.EpaAlgorithm;
import it.gius.pe.algorithm.gjk.GjkEpaAlgorithm;
import it.gius.pe.algorithm.gjk.NotConvergingException;
import it.gius.pe.algorithm.gjk.Simplex.SimplexEdge;
import it.gius.pe.algorithm.gjk.shapes.GjkEllipse;
import it.gius.pe.algorithm.gjk.shapes.GjkMinkDiffShape;
import it.gius.pe.algorithm.gjk.shapes.GjkPolygon;
import it.gius.pe.mains.MainMovingTest;
import it.gius.processing.util.MyAbstractPApplet;
import processing.core.PApplet;

@SuppressWarnings("serial")
public class GjkFinalApplet extends MyAbstractPApplet {


	private double mediumIterationPP = 0;
	
	private long numComparisonPP = 0;
	
	private double mediumIterationEP = 0;
	private long numComparisonEP = 0;
	


	private Polygon[] arrayPoly = null;
	private Ellipse[] ellipseArray = null;
	private Semaphore mutex = null;
	private GjkMinkDiffShape[][] minkDiffShape;
	private GjkMinkDiffShape[] minkDiffpolyFirstEllipse;
	boolean colliding[] = null;

	public GjkFinalApplet(Polygon[] arrayPoly) {
		this.arrayPoly = arrayPoly;
		colliding = new boolean[arrayPoly.length];
	}

	public GjkFinalApplet(Polygon[] arrayPoly,Ellipse[] ellipseArray,Semaphore mutex) {
		this.arrayPoly = arrayPoly;
		this.ellipseArray = ellipseArray;
		this.mutex = mutex;
		colliding = new boolean[arrayPoly.length];
		minkDiffShape = new GjkMinkDiffShape[arrayPoly.length][arrayPoly.length];
		
		minkDiffpolyFirstEllipse = new GjkMinkDiffShape[arrayPoly.length];

	}

	public void setup(){


		cursor(PApplet.CROSS);

		size(MainMovingTest.sizeX,MainMovingTest.sizeY);
		//frameRate(15);
		if(!goStop.isGo())
			noLoop();

	}
	
	public double getMediumIterationPP() {
		return mediumIterationPP;
	}

	public double getMediumIterationEP() {
		return mediumIterationEP;
	}


	private DistanceSolution sol = new DistanceSolution();



	public void draw()
	{

		//println("draw");
		background(255);


		strokeWeight(2);
		stroke(0);
		text(""+mouseX + ", " + mouseY, width-60, height-20);
		strokeWeight(1);


		/*if(mutex !=null)
			try {
				mutex.acquire();
			} catch (InterruptedException e) {
				e.printStackTrace();
				return;
			}
*/
			/*for(int i=0;i < arrayPoly.length; i++)
				arrayPoly[i].move();*/


			for(int i=0; i<colliding.length;i++)
				colliding[i] = false;


			for(int i= 0;i < arrayPoly.length; i++)
			{

				for(int j=i+1; j<arrayPoly.length;j++)
				{

					/*try {

						if(!BoundaryBox.overlap(arrayPoly[i].getBoundaryBox(),arrayPoly[j].getBoundaryBox()))
							continue;
					} catch (BadShapeException e1) {
						e1.printStackTrace();
					}	*/

					if(minkDiffShape[i][j] == null)
						minkDiffShape[i][j] = new GjkMinkDiffShape(new GjkPolygon(arrayPoly[i]),new GjkPolygon(arrayPoly[j]));

					try {
						//GjkAlgorithm.distance(arrayPoly[i], arrayPoly[j], sol,sol.indexPoly1PointInSolution,sol.indexPoly2PointInSolution);
						GjkEpaAlgorithm.distance(minkDiffShape[i][j],SystemCostants.ORIGIN2D,sol);
						//println("sol.indexPoly1PointInSolution: " + sol.indexPoly1PointInSolution);
						//println("sol.indexPoly2PointInSolution: " + sol.indexPoly2PointInSolution);
						stroke(210,210,0);
						line(sol.p1.x,sol.p1.y,sol.p2.x,sol.p2.y);
						fill(0,0,0);
						ellipse(sol.p1.x,sol.p1.y,5,5);
						ellipse(sol.p2.x,sol.p2.y,5,5);

						if(sol.distance >= -SystemCostants.EPSILON  && sol.distance < SystemCostants.EPSILON)
						{
							System.out.println(sol.distance);
							colliding[i] = true;
							colliding[j] = true;
						}
						
						if(sol.penetration)
						{
							/*SimplexEdge edgeSolution = new SimplexEdge();
							EpaAlgorithm.penetrationNormalAndDepth(minkDiffShape[i][j], sol.lastSimplex,
									SystemCostants.ORIGIN2D, edgeSolution);*/
							
							System.out.println("interpenetration between: " +i+ " and " +j);
							System.out.println("penetration depth: " + sol.penetrationDepth);
							System.out.println("penetration normal: " + sol.normalPenetration);
							//System.out.println("penetration iterations: " + edgeSolution.iterations);
							stroke(0,0,220);
							line(sol.p1.x,sol.p1.y,sol.p1.x + 15*sol.normalPenetration.x,sol.p1.y + 15*sol.normalPenetration.y);
						}
						
						mediumIterationPP = mediumIterationPP*numComparisonPP + sol.iterationCount;
						numComparisonPP++;
						mediumIterationPP = mediumIterationPP/numComparisonPP;

					} catch (NotConvergingException e) {
;						e.printStackTrace();
						println("center firt poly: " + arrayPoly[0].center);
						println("center second poly: " + arrayPoly[1].center);
						goStop.setGo(false);
					}


				}


				fill(0,0,210);
				stroke(10);
				/*println("sol.distance: " + sol.distance);
				println("center firt poly: " + arrayPoly[0].getCenter());
				println("center second poly: " + arrayPoly[1].getCenter());*/
				if(colliding[i])
				{
					//if(sol.distance ==0)
					fill(210,0,0);
					stroke(210,0,0);
				}


				noFill();
				drawPoly(arrayPoly[i],i);

			}

			boolean ellipseColliding = false;

			for( int i=0; i<ellipseArray.length;i++)
			{
				for(int j=0; j<arrayPoly.length;j++)
				{
					if(minkDiffpolyFirstEllipse[j] == null)
						minkDiffpolyFirstEllipse[j] = new GjkMinkDiffShape(new GjkEllipse(ellipseArray[i]),new GjkPolygon(arrayPoly[j]));
					//GjkMinkDiffShape minkDiffEllipse = new GjkMinkDiffShape(new GjkEllipse(ellipseArray[i]),new GjkPolygon(arrayPoly[j]));
					try {
						GjkEpaAlgorithm.distance(minkDiffpolyFirstEllipse[j],SystemCostants.ORIGIN2D,sol);

						if(sol.distance == 0)
							ellipseColliding = true;

						stroke(210,210,0);
						line(sol.p1.x,sol.p1.y,sol.p2.x,sol.p2.y);
						fill(0,0,0);
						ellipse(sol.p1.x,sol.p1.y,5,5);
						ellipse(sol.p2.x,sol.p2.y,5,5);
						
						if(sol.penetration)
						{
							/*SimplexEdge edgeSolution = new SimplexEdge();
							EpaAlgorithm.penetrationNormalAndDepth(minkDiffpolyFirstEllipse[j], sol.lastSimplex,
									SystemCostants.ORIGIN2D, edgeSolution);*/
							
							System.out.println("interpenetration between: " +i+ " and " +j);
							System.out.println("penetration depth: " + sol.penetrationDepth);
							System.out.println("penetration normal: " + sol.normalPenetration);
							//System.out.println("penetration iterations: " + edgeSolution.iterations);
							stroke(0,0,220);
							line(sol.p1.x,sol.p1.y,sol.p1.x + 15*sol.normalPenetration.x,sol.p1.y + 15*sol.normalPenetration.y);
						}
						
						mediumIterationEP = mediumIterationEP*numComparisonEP + sol.iterationCount;
						numComparisonEP++;
						mediumIterationEP = mediumIterationEP/numComparisonEP;
						
						//println("iteration for ellipse polygon"+j +": " + sol.iterationCount);
					} catch (NotConvergingException e) {
						e.printStackTrace();
						println("not convergin ellipse and poly " +j);
					}
				}
				AABoundaryBox ellBox = new AABoundaryBox();
				try {
					ellipseArray[i].getBoundaryBox(ellBox);
					noFill();
					stroke(0,0,230);
					rect(ellBox.lowerBound.x,ellBox.lowerBound.y,
							ellBox.upperBound.x-ellBox.lowerBound.x, ellBox.upperBound.y-ellBox.lowerBound.y);
				} catch (BadShapeException e) {
					e.printStackTrace();
				}
				noFill();
				stroke(0);
				if(ellipseColliding)
					stroke(220,0,0);

				drawEllipse(ellipseArray[i]);
			}


	/*		if(mutex !=null)
				mutex.release();*/




	}

	private int indexPolyDrag = -1;
	private boolean dragging = false;
	private boolean dragEllipse = false;

	@Override
	public void mousePressed() {


		DistanceSolution sol = new DistanceSolution();
		for(int i= 0; i <arrayPoly.length;i++)
		{
			try {
				GjkEpaAlgorithm.distance(new GjkPolygon(arrayPoly[i]), new Vec2(mouseX,mouseY), sol);

				if(sol.distance <= 0)
				{
					indexPolyDrag = i;
					dragging = true;
					if(!goStop.isGo())
						goStop.setGo(true);

					return;
				}

			} catch (NotConvergingException e) {
				e.printStackTrace();
			}
		}

		try {
			GjkEpaAlgorithm.distance(new GjkEllipse(ellipseArray[0]), new Vec2(mouseX,mouseY), sol);

			if(sol.distance <= 0)
			{
				dragging = true;
				dragEllipse = true;
				if(!goStop.isGo())
					goStop.setGo(true);

				return;
			}
			
			//println("iteration for ellipse mouse: " + sol.iterationCount);
		} catch (NotConvergingException e) {
			e.printStackTrace();
			println("mouseX: " + mouseX);
			println("mouseY: " + mouseY);
		}


	}

	
	/*public void mouseMoved() {
		if(!goStop.isGo())
			redraw();
	}*/

	@Override
	public void mouseDragged() {
		if(dragging)
		{
			if(dragEllipse)
				ellipseArray[0].centerPosition.set(mouseX,mouseY);
			else
				arrayPoly[indexPolyDrag].setPosition(new Vec2(mouseX,mouseY));

		}
	}

	@Override
	public void mouseReleased() {
		dragging = false;
		indexPolyDrag = -1;
		dragEllipse = false;
		goStop.setGo(false);
	}

	private void drawEllipse(Ellipse ellipse)
	{
		pushMatrix();
		translate(ellipse.centerPosition.x, ellipse.centerPosition.y);
		rotate(ellipse.angle);

		ellipse(0, 0, 2*ellipse.getA(), 2*ellipse.getB());

		popMatrix();
	}

	private void drawPoly(Polygon poly,int index)
	{
		fill(0);
		for(int i=0; i< poly.getVerticesCount(); i++)
		{
			Vec2 v = poly.getVertex(i);
			text(i,v.x-3,v.y);
		}

		noFill();
		beginShape();
		for(int i=0; i< poly.getVerticesCount(); i++)
		{
			Vec2 v = poly.getVertex(i);
			vertex(v.x,v.y);
		}
		endShape(PApplet.CLOSE);

		fill(0);
		text(index,poly.center.x,poly.center.y);
	}

}
